#include "debug.h"

/*
 * C Function wrapper for TRAP #15 function to output a character
 */
VOID debug_out_char( CHAR c )
{
    /* Store registers */
    asm( "move.l %d0, -(%a7)" );
    asm( "move.l %d1, -(%a7)" );

    /* Load CHAR c into d1 */
    asm( "move.l 8(%a6), %d1" );

    /* Setup trap function */
    asm( "move.l #0x13, %d0" );
    asm( "trap #15" );

    /* Restore registers  */
    asm(" move.l %d1, (%a7)+" );
    asm(" move.l %d0, (%a7)+" );
}


/*
 * Print a C-style null terminated string
 */
SINT32 debug_out_string( CHAR* s )
{
    if ( s == NULL )
    {
        return RTX_ERROR;
    }
    while ( *s != '\0' )
    {
        debug_out_char( *s++ );
    }
    return RTX_SUCCESS;
}

/*
 * Print a C-style null terminated string
 */
SINT32 debug_out_int( UINT32 num )
{
    int i=0;
	int j=0;
	int ones=0;
	char temp[5]; //5=num digits in 32676

	if(num == 0) {
		debug_out_char((char)(48));
	} else {
		while (num!=0)
		{
			ones=num%10; //get current ones digit
			temp[i]=(char)(ones+48); //48=(int)'0';
			num=num/10; //remove current ones digit
			i++; //length of number
		}

		for(j=i-1;j>=0;j--)
		{
			debug_out_char(temp[j]); //reorder string correctly
		}
	}

    return RTX_SUCCESS;
}

/*
 * Checks if a character is available in the input buffer.
 */


//BOOLEAN debug_char_present()
//{
//	/* Check for input char */
//	asm( "move.l #0x14, %d0" );
//	asm( "trap #15" );
//}


/*
 * Grabs the next character from the input buffer.
 */
//CHAR debug_in_char()
//{
//	/* Get input character */
//	asm( "move.l #0x10, %d0" );
//	asm( "trap #15" );
//
//	/* Move character to return value */
//	asm( "move.l %d1, %d0" );
//}

/*
 * Takes in a string from the input buffer.
 */
//CHAR* debug_in_string()
//{
//	CHAR* s;
//
//	while( debug_char_present() ) {
//		*s = debug_in_char();
//		*s++;
//	}
//	*s = '\0';
//
//	return s;
//}
